#include <atomic>
#include <string>
#include <thread>
#include <cstdio>

#include "AEyeSDKConfig.hpp"
#include "Sensor.hpp"
#include "PointCloudFrame.hpp"

void print_help() {
  printf("aeyeSensorController - An example for using the AEye Sensor SDK.\n\n");
}
void print_usage() {
  printf("USAGE: ./aeyeSensorController <sensor_ip> <network_interface>\n");
}

void processPointCloudDataCallback(aeye::PointCloudFrame&& point_cloud_frame);

static std::atomic<size_t> count {0};
static constexpr size_t MAX_FRAMES {101};

int main(int argc, char **argv) {
  aeye::InitializeLoggerToVoid();

  if (argc == 1) {
    print_help();
    print_usage();
    return 1;
  }

  if (argc != 3) {
    printf("Received incorrect number of arguments.\n\n");
    print_usage();
    return 1;
  }

  std::string sensor_ip = argv[1];
  std::string net_interface = argv[2];

  aeye::Sensor sensor{
    aeye::SensorConfig{}, // Initialize the Sensor object with default configuration values.
    aeye::DataPortConfig{7041}, // The default values for these objects can be overridden, with custom ports in this case.
    net_interface,
    sensor_ip,
    "SensorControllerExample", // Give the sensor a human-readable name to identify it.
  };

  // Set up a point cloud callback to handle pointcloud frames as they come in.
  sensor.setOnPointCloudFrameCallback(&processPointCloudDataCallback);

  // Establish a connection to the sensor - note that this does not start sensing yet.
  // This function also syncs the sensor's clock to the local machine's.
  sensor.connect();

  // Select which scan pattern to use.
  sensor.sendShotlistFileNumber(0);

  // You can fetch the name of the selected scan pattern from the Sensor object as well.
//  auto name = sensor.getShotlistName(1);
//  printf("Running with scan pattern: %s.\n", name.c_str());

  // Activate the sensor.
  sensor.start();

  // Wait for the callbacks to be processed in other threads.
  while (count < MAX_FRAMES) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }

  // Note that the Sensor class is an RAII object, and it will perform its own cleanup when it goes out of scope,
  // including stopping the connected sensor and closing the connection.
  //
  // You may still choose to explicitly stop a Sensor if you want to stop transmission without destroying the object
  // by calling stop().

//  sensor.stop();

  return 0;
}

void processPointCloudDataCallback(aeye::PointCloudFrame&& point_cloud_frame) {
  // Note that the callback function may take more time to run than it takes to receive the next frame,
  // so locking and accessing shared resources is recommended in most cases.

  // Ignore a potential empty frame at the start of transmission.
  const int frame_id = point_cloud_frame.getFrameId();
  if (frame_id == 0) return;

  // Uncomment this if you want to print the point cloud data
  // for(auto &point : point_cloud_frame.getPoints()) {
  //   printf("\n Point: X: %f\tY: %f\tZ: %f\tIntensity: %u\t",point.pointReturnCartesianCoords.x,point.pointReturnCartesianCoords.y,point.pointReturnCartesianCoords.z,point.pointReturnIntensity.intensity_type_data);
  // }
  size_t i = count++;
  // Additionally ignore potential trailing frames.
  if (i >= MAX_FRAMES) return;

  if (i % 10 == 0) {
    printf("Received frame #%zu in sequence, with frame ID #%i.\n", i, frame_id);
  }

  // There is further processing of point cloud data can be done, but that would be out of scope for this example.


}
